// ****************************************************************
// FrSky telemetry
// Version: 0.2.1
// Date 20/09/2012
// Changes: V0.2.1: - make it work with 2.1 (shared dev)
// Date: 14/08/2012
// Changes: V0.2: - Byte stuffing added
//                - vBat will be send, if "#define FAS_100" is comment out
//                V0.1: - First release
// ****************************************************************

#if defined(TELEMETRY_FRSKY)
  
  #define TX_BUFFER_SIZE 128

  static volatile uint8_t headTX,tailTX;
  static uint8_t bufTX[TX_BUFFER_SIZE];
  static volatile uint8_t serialHeadRX[0],serialTailRX[0];
  static uint8_t checksum;

  // Set UART Interrupt
  ISR(USART_UDRE_vect) {
    uint8_t t = tailTX;
    if (headTX != t) {
      if (++t >= TX_BUFFER_SIZE) t = 0;
      UDR0 = bufTX[t];  // Transmit next byte in the ring
      tailTX = t;
    }
    if (t == headTX) UCSR0B &= ~(1<<UDRIE0); // Check if all data is transmitted . if yes disable transmitter UDRE interrupt
  }

  // user defines
  //#define FAS_100  //if commment out, MultiWii vBat voltage will be send instead of FrSky FAS 100 voltage

  // Serial config datas
  #define TELEMETRY_FRSKY_BAUD 9600  

  // Timing
  #define Time_telemetry_send 125000 
  static uint8_t cycleCounter = 0;
  static uint32_t FrSkyTime  = 0;

  // Frame protocol
  #define Protocol_Header   0x5E
  #define Protocol_Tail      0x5E
  
  // Data Ids  (bp = before point; af = after point)
  // Official data IDs
  #define ID_GPS_altidute_bp    0x01
  #define ID_GPS_altidute_ap    0x09
  #define ID_Temprature1        0x02
  #define ID_RPM                0x03
  #define ID_Fuel_level         0x04
  #define ID_Temprature2        0x05
  #define ID_Volt               0x06
  #define ID_Altitude_bp        0x10
  #define ID_Altitude_ap        0x21
  #define ID_GPS_speed_bp       0x11
  #define ID_GPS_speed_ap       0x19
  #define ID_Longitude_bp       0x12
  #define ID_Longitude_ap       0x1A
  #define ID_E_W                0x22
  #define ID_Latitude_bp        0x13
  #define ID_Latitude_ap        0x1B
  #define ID_N_S                0x23
  #define ID_Course_bp          0x14
  #define ID_Course_ap          0x24
  #define ID_Date_Month         0x15
  #define ID_Year               0x16
  #define ID_Hour_Minute        0x17
  #define ID_Second             0x18
  #define ID_Acc_X              0x24
  #define ID_Acc_Y              0x25
  #define ID_Acc_Z              0x26
  #define ID_Voltage_Amp_bp     0x3A
  #define ID_Voltage_Amp_ap     0x3B
  #define ID_Current            0x28
  // User defined data IDs
  #define ID_Gyro_X             0x40
  #define ID_Gyro_Y             0x41
  #define ID_Gyro_Z             0x42
  // RSSI values
  #define ID_RSSI_Rx            0xF7
  #define ID_RSSI_Tx            0xF6
  #define ID_Link_Head          0x7E
  #define ID_Link               0xFE

   void configure_FrSky()
   {         
      SerialOpen(0, TELEMETRY_FRSKY_BAUD);      
   }
   
   // Main function FrSky telemetry
   void telemetry_frsky() {
     
      if (currentTime > FrSkyTime ) {
        
         FrSkyTime = currentTime + Time_telemetry_send;
         cycleCounter++;
         
         // Datas sent every 125 ms
         //send_Accel();
         send_Link_Quality();
         //sendDataTail();   
         /*
         if ((cycleCounter % 4) == 0) 
         {      
            // Datas sent every 500ms
            send_Altitude();
            send_RPM();
            send_Course();
            //send_GPS_speed();
            //send_Cell_volt();       todo
            sendDataTail();

         }
         
         if ((cycleCounter % 8) == 0) 
         {      
            // Datas sent every 1s            
            send_Time();
            //send_GPS_position();
            //send_GPS_altitude();
            //send_Temperature2();  // Distance_to_home
            send_Fuel_level();
            //send_Voltage_ampere();
            //send_Temperature1();  // num of Sats
            sendDataTail();            
         }
         
         if (cycleCounter == 40) 
         {
            // Datas sent every 5s
            cycleCounter = 0;       
         }
         */
      }
   }

   void write_FrSky8(uint8_t Data)
   {
      SerialWrite(Data);
   }

   void write_FrSky16(uint16_t Data)
   {
      uint8_t Data_send;
      Data_send = Data;      
      check_FrSky_stuffing(Data_send);
      Data_send = Data >> 8 & 0xff;
      check_FrSky_stuffing(Data_send);
   }
   
   void check_FrSky_stuffing(uint8_t Data) //byte stuffing
   {
      if (Data == 0x5E)   
      {
         write_FrSky8(0x5D);
         write_FrSky8(0x3E);
      }
      else if (Data == 0x5D)   
      {
         write_FrSky8(0x5D);
         write_FrSky8(0x3D);
      }
      else
      {
         write_FrSky8(Data);         
      }
   }

   static void sendDataHead(uint8_t Data_id)
   {
      write_FrSky8(Protocol_Header);
      write_FrSky8(Data_id);
   }

   static void sendDataTail(void)
   {
      write_FrSky8(Protocol_Tail);      
   }


   //*********************************************************************************
   //-----------------   Telemetry Data   --------------------------------------------   
   //*********************************************************************************
   /*
   // GPS altitude
   void send_GPS_altitude(void)
   {          
      if (f.GPS_FIX && GPS_numSat >= 4) 
      {
         int16_t Datas_GPS_altidute_bp;
         uint16_t Datas_GPS_altidute_ap;

         Datas_GPS_altidute_bp = GPS_altitude;
         Datas_GPS_altidute_ap = 0;

         sendDataHead(ID_GPS_altidute_bp);
         write_FrSky16(Datas_GPS_altidute_bp);
         sendDataHead(ID_GPS_altidute_ap);
         write_FrSky16(Datas_GPS_altidute_ap);
      }
   }
   
   // Temperature
   void send_Temperature1(void)
   {
      int16_t Datas_Temprature1;

      Datas_Temprature1 = GPS_numSat;  // Number of Satalits alias Temp1

      sendDataHead(ID_Temprature1);
      write_FrSky16(Datas_Temprature1); 
   }
*/
   // RPM
   void send_RPM(void)
   {
      uint16_t Datas_RPM = 0;      
      for (uint8_t i=0;i<4;i++)
      {
         Datas_RPM += 100;
      } 
      Datas_RPM = (Datas_RPM / 8) / 30;   // RPM 

      sendDataHead(ID_RPM);
      write_FrSky16(Datas_RPM);
   }

   // Fuel level
   void send_Fuel_level(void)
   {
      uint16_t Datas_Fuel_level;

      Datas_Fuel_level = 0; 

      sendDataHead(ID_Fuel_level);
      write_FrSky16(Datas_Fuel_level); 
   }

   // Temperature 2
   void send_Temperature2(void)
   {
     /* 
     if (f.GPS_FIX_HOME)
      {
         int16_t Datas_Temprature2;

         Datas_Temprature2 = GPS_distanceToHome; // Distance to home alias Temp2

         sendDataHead(ID_Temprature2);
         write_FrSky16(Datas_Temprature2);  
      }
    */      
   }

   // Cell voltage  todo !!!!!!!!!!!!!!!!!!
   void send_Cell_volt(void) // Datas FrSky FLVS-01 voltage sensor
   {
      uint16_t Datas_Volt;
      uint8_t number_of_cells = 0;   // LiPo 3S = 3; LiPo 4S = 4 ...
      static uint8_t cell = 0;
      if (cell >= number_of_cells); cell = 0;
      
      Datas_Volt = 0; // 0.01v / 0 ~ 4.2v

      sendDataHead(ID_Volt);
      write_FrSky16(Datas_Volt); 
   }

   // Altitude
   void send_Altitude(void)
   {
      uint16_t Datas_altitude_bp;
      uint16_t Datas_altitude_ap;
      static uint16_t Start_altitude;

      //if (!f.ARMED)
      //{
      //   Start_altitude = EstAlt / 100;
      //}

      Datas_altitude_bp = (1000 / 100) - 20;
      Datas_altitude_ap = (1000 % 100);

      sendDataHead(ID_Altitude_bp);
      write_FrSky16(Datas_altitude_bp);
      sendDataHead(ID_Altitude_ap);
      write_FrSky16(Datas_altitude_ap);
   }

   // GPS speed
   void send_GPS_speed(void)
   {
      /*
      if (f.GPS_FIX && GPS_numSat >= 4) 
      {            
         uint16_t Datas_GPS_speed_bp;
         uint16_t Datas_GPS_speed_ap;
         
         Datas_GPS_speed_bp = GPS_speed * 0.036;
         Datas_GPS_speed_ap = 0;
      
         sendDataHead(ID_GPS_speed_bp);
         write_FrSky16(Datas_GPS_speed_bp);
         sendDataHead(ID_GPS_speed_ap);
         write_FrSky16(Datas_GPS_speed_ap);
      }
      */
   }

   // GPS position
   void send_GPS_position(void)
   {
   
         uint16_t Datas_Longitude_bp;
         uint16_t Datas_Longitude_ap;
         uint16_t Datas_E_W;
         uint16_t Datas_Latitude_bp;
         uint16_t Datas_Latitude_ap;
         uint16_t Datas_N_S;
         //Datas_Longitude_bp = abs(GPS_coord[LON]) / 100000;
         Datas_Longitude_bp = 0;
         //Datas_Longitude_ap = abs((GPS_coord[LON])/10)  % 10000;
         Datas_Longitude_ap = 0;
         Datas_E_W =  'E';
         Datas_Latitude_bp = 0;
         Datas_Latitude_ap = 0;
         Datas_N_S = 'N';

         sendDataHead(ID_Longitude_bp);
         write_FrSky16(Datas_Longitude_bp);
         sendDataHead(ID_Longitude_ap);
         write_FrSky16(Datas_Longitude_ap);
         sendDataHead(ID_E_W);
         write_FrSky16(Datas_E_W);

         sendDataHead(ID_Latitude_bp);
         write_FrSky16(Datas_Latitude_bp);
         sendDataHead(ID_Latitude_ap);
         write_FrSky16(Datas_Latitude_ap);
         sendDataHead(ID_N_S);
         write_FrSky16(Datas_N_S);
      
   }

   // Course
   void send_Course(void)
   {
      uint16_t Datas_Course_bp;
      uint16_t Datas_Course_ap;

      Datas_Course_bp = 12;
      Datas_Course_ap = 0;

      sendDataHead(ID_Course_bp);
      write_FrSky16(Datas_Course_bp);
      sendDataHead(ID_Course_ap);
      write_FrSky16(Datas_Course_ap);
   }
   
   // Time
   void send_Time(void)
   {
      uint32_t seconds_since_start = millis() / 1000;
            
      uint16_t Datas_Date_month;
      uint16_t Datas_Year;
      uint16_t Datas_Minutes_hours;
      uint16_t Datas_seconds;
            
      Datas_Date_month = 0;
      Datas_Year = 12;
      Datas_Minutes_hours = (seconds_since_start / 60) % 60;
      Datas_seconds = seconds_since_start % 60;      
      
      sendDataHead(ID_Hour_Minute);
      write_FrSky16(Datas_Minutes_hours);
      sendDataHead(ID_Second);
      write_FrSky16(Datas_seconds);
   }
   /*
   // ACC
   void send_Accel(void)
   {
      int16_t Datas_Acc_X;
      int16_t Datas_Acc_Y;
      int16_t Datas_Acc_Z;

      Datas_Acc_X = ((float)accSmooth[0] / acc_1G) * 1000;
      Datas_Acc_Y = ((float)accSmooth[1] / acc_1G) * 1000;
      Datas_Acc_Z = ((float)accSmooth[2] / acc_1G) * 1000;

      sendDataHead(ID_Acc_X);
      write_FrSky16(Datas_Acc_X);
      sendDataHead(ID_Acc_Y);
      write_FrSky16(Datas_Acc_Y);
      sendDataHead(ID_Acc_Z);
      write_FrSky16(Datas_Acc_Z);      
   }

   // Voltage (Ampere Sensor)  
   void send_Voltage_ampere(void)
   {
   
      #if defined (FAS_100)   // todo   !!!!!!!!!!!!!!!!!
      {
         uint16_t Datas_Voltage_Amp_bp;
         uint16_t Datas_Voltage_Amp_ap;
         uint16_t Datas_Current;   

         Datas_Voltage_Amp_bp = 0;
         Datas_Voltage_Amp_ap = 0;   
         Datas_Current = 0;

         sendDataHead(ID_Voltage_Amp_bp);
         write_FrSky16(Datas_Voltage_Amp_bp);
         sendDataHead(ID_Voltage_Amp_ap);
         write_FrSky16(Datas_Voltage_Amp_ap);   
         sendDataHead(ID_Current);
         write_FrSky16(Datas_Current);
      }
      #else   // use vBat
      {
         uint16_t Datas_Voltage_vBat_bp;
         uint16_t Datas_Voltage_vBat_ap;   
         uint16_t voltage;
         voltage = (vbat * 110) / 21;          
         Datas_Voltage_vBat_bp = voltage / 100;
         Datas_Voltage_vBat_ap = ((voltage % 100) + 5) / 10;         

         sendDataHead(ID_Voltage_Amp_bp);
         write_FrSky16(Datas_Voltage_vBat_bp);
         sendDataHead(ID_Voltage_Amp_ap);
         write_FrSky16(Datas_Voltage_vBat_ap);   
      }
      #endif
   }
  */

  void send_RSSI(void) {
      
      uint16_t Datas_RSSI_Rx;
      uint16_t Datas_RSSI_Tx;
      
      Datas_RSSI_Rx = teledata.RSSI;
      Datas_RSSI_Tx = rssi; 
      
      sendDataHead(ID_RSSI_Rx);
      write_FrSky16(Datas_RSSI_Rx);
      
      sendDataHead(ID_RSSI_Tx);
      write_FrSky16(Datas_RSSI_Tx);
  }
  
  // Sending A1 / A2 / rssiRX / rssiTX
  void send_Link_Quality(void) {

      write_FrSky8(ID_Link_Head);
      write_FrSky8(ID_Link);
      write_FrSky8(116);
      write_FrSky8(139);
      write_FrSky8(rssi / 2);
      write_FrSky8(teledata.RSSI); // The protocol sends this as 2x so no need to divide
      write_FrSky8(0);
      write_FrSky8(0);
      write_FrSky8(0);
      write_FrSky8(0);
      write_FrSky8(ID_Link_Head);
  }
  
  void serialize8(uint8_t a) {
    uint8_t t = headTX;
    if (++t >= TX_BUFFER_SIZE) t = 0;
    bufTX[t] = a;
    checksum ^= a;
    headTX = t;
  }

  void SerialWrite(uint8_t c){
      serialize8(c);
      // Serial0 TX is driven via a buffer and a background intterupt
      // enable transmitter UDRE interrupt if deactivacted
      UCSR0B |= (1<<UDRIE0); 
  }
  
  static void inline SerialOpen(uint8_t port, uint32_t baud) {
    uint8_t h = ((F_CPU  / 4 / baud -1) / 2) >> 8;
    uint8_t l = ((F_CPU  / 4 / baud -1) / 2);
    UCSR0A  = (1<<U2X0); 
    UBRR0H = h; 
    UBRR0L = l; 
    UCSR0B |= (1<<RXEN0)|(1<<TXEN0)|(1<<RXCIE0); 
  }
  
#endif 
